// This sketch reads IMU data from the SparkFun 5DOF IMU found at:
// http://www.sparkfun.com/commerce/product_info.php?products_id=741
// The Kalman filter implementation is directly taken from the work
// of Tom Pycke at: http://tom.pycke.be/mav/90/sparkfuns-5dof
// The AREF pin is connected to 3.3V to better use the 10 bit A/D
// Roll left increase Y gyro voltage, roll right decreases Y gyro voltage
// Pitch down decreases X gyro voltage, pitch up increases X gyro voltage

#include <math.h>

// Debug data 
#define BAUD 38400  

// Define pin assignments
#define LEDPIN 13
#define XACCELPIN 2 // I miswired my board, change order based on your setup
#define YACCELPIN 0
#define ZACCELPIN 1
#define XRATEPIN 4
#define YRATEPIN 3

// Define sensor measurements
static const float gyro_pitch_rad_per_second = 8.72*2.0;
static const float gyro_roll_rad_per_second = 8.72*2.0;

// Kalman filter
float tmp, roll_angle;
float acc_gyro = 0, dt;
struct Gyro1DKalman {
	float x_angle, x_bias;
	float P_00,  P_01, P_10, P_11;	
	float Q_angle, Q_gyro;
	float R_angle;
};
struct Gyro1DKalman filter_roll;
struct Gyro1DKalman filter_pitch;

unsigned long previousTime = 0;
unsigned long currentTime = 0;

void setup() {
  digitalWrite(LEDPIN, HIGH);
  analogReference(EXTERNAL); // Current external ref is 3.3V
  Serial.begin(BAUD);
  init_Gyro1DKalman(&filter_roll, 0.0001, 0.0003, 0.69);
  previousTime = millis();
}

void loop() {
  currentTime = millis();
  dt = (currentTime - previousTime) / 1000;
  previousTime = currentTime;
  
  tmp = PredictAccG_roll(accelero_z(), accelero_y(), accelero_x());
  ars_predict(&filter_roll, gyro_roll_rad(), dt);    // Kalman predict
  roll_angle = ars_update(&filter_roll, tmp);        // Kalman update + result (angle) 

  digitalWrite(LEDPIN, HIGH);
  Serial.print(analogRead(YACCELPIN));
  Serial.print(",");
  Serial.print((int)tmp);
  Serial.print(",");
  Serial.println((int)roll_angle);
  digitalWrite(LEDPIN, LOW);  
}

float PredictAccG_roll(float a_z, float a_y, float a_x) {
	return -(atan2(-a_z, a_y)-(3.14159/2.0));
}

// this axis should be towards the nose of the plane
float accelero_x() {
	return (float)((long)analogRead(XACCELPIN) - 33000) / (39600.0-33000.0);
}

// this axis should be towards the right wingtip
float accelero_y() {
	return (float)((long)analogRead(YACCELPIN) - 32800) / (39400.0-32800.0);
}

// this axis should be towards the ground
float accelero_z() {
	return (float)((long)analogRead(ZACCELPIN) - 32300) / (32300.0-25980.0);
}

float gyro_roll_rad() {
	return -(((float)analogRead(XRATEPIN) * 0.0004394279)-13.09);
}

float gyro_pitch_rad() {
	return -(((float)analogRead(YRATEPIN) * 0.0004394279)-13.09);
}

void init_Gyro1DKalman(struct Gyro1DKalman *filterdata, float Q_angle, float Q_gyro, float R_angle) {
	filterdata->Q_angle = Q_angle;
	filterdata->Q_gyro  = Q_gyro;
	filterdata->R_angle = R_angle;
}

void ars_predict(struct Gyro1DKalman *filterdata, const float dotAngle, const float dt) {
	filterdata->x_angle += dt * (dotAngle - filterdata->x_bias);
	filterdata->P_00 +=  - dt * (filterdata->P_10 + filterdata->P_01) + filterdata->Q_angle * dt;
	filterdata->P_01 +=  - dt * filterdata->P_11;
	filterdata->P_10 +=  - dt * filterdata->P_11;
	filterdata->P_11 +=  + filterdata->Q_gyro * dt;
}

float ars_update(struct Gyro1DKalman *filterdata, const float angle_m) {
	const float y = angle_m - filterdata->x_angle;
	const float S = filterdata->P_00 + filterdata->R_angle;
	const float K_0 = filterdata->P_00 / S;
	const float K_1 = filterdata->P_10 / S;
	
	filterdata->x_angle +=  K_0 * y;
	filterdata->x_bias  +=  K_1 * y;
	filterdata->P_00 -= K_0 * filterdata->P_00;
	filterdata->P_01 -= K_0 * filterdata->P_01;
	filterdata->P_10 -= K_1 * filterdata->P_00;
	filterdata->P_11 -= K_1 * filterdata->P_01;

	return filterdata->x_angle;
}
